package agent;

import instanceXMLParser.Instance;

import java.util.ArrayList;
import java.util.List;

import org.jgrapht.DirectedGraph;
import org.jgrapht.alg.DijkstraShortestPath;
import org.jgrapht.alg.FloydWarshallShortestPaths;
import org.jgrapht.graph.DefaultDirectedGraph;
import org.jgrapht.graph.DefaultEdge;
import org.paukov.combinatorics.Factory;
import org.paukov.combinatorics.Generator;
import org.paukov.combinatorics.ICombinatoricsVector;

import aima.core.agent.Action;
import aima.core.agent.impl.DynamicAction;
import aima.core.agent.impl.NoOpAction;
import core.VacuumEnvironment.LocationState;

public class VacuumAgentSolverHeuristicKBest extends AbstractVacuumSolver
implements Runnable {

	DirectedGraph<Integer, DefaultEdge> board = new DefaultDirectedGraph<Integer, DefaultEdge>(
			DefaultEdge.class);

	Instance instance;

	List<Integer> targetCells = new ArrayList<Integer>();

	final int INFDIST = 1000;

	public int KBEST = 5;

	int solutionNumCleaned;

	double solutionCost;

	List<Integer> solutionPath = new ArrayList<Integer>();

	List<Integer> completeSolutionPath = new ArrayList<Integer>();

	//FloydWarshallShortestPaths<Integer, DefaultEdge> shortesTpath;

	List<Action> solution;

	int [][] distances;

	public VacuumAgentSolverHeuristicKBest(int KBEST) {
		this.KBEST = KBEST;
		id = "kbest" + KBEST;
	}

	public VacuumAgentSolverHeuristicKBest() {
	}


	private int shortestDistance(Integer i, Integer j) {
		if(i==j){
			return 0;
		}
		
		Integer res = distances[i][j];
		if(res==-1){
			List<DefaultEdge> path=DijkstraShortestPath.findPathBetween(board, i, j);
			if(path==null){
				distances[i][j]=INFDIST;
			}else{
				distances[i][j]=path.size();
			}
		}
		return distances[i][j];
	
	}



	public void solve(String path) {
		long startTime = System.currentTimeMillis();

		instance = new Instance();
		instance.buildINstanceJDom(path);
		//		instance.toString();
		board = createGraph();
		
		
	//	shortesTpath = new FloydWarshallShortestPaths<Integer, DefaultEdge>(				board);
		targetCells = listAllreachableTarget();
		//		System.out.println("Target cells: " + targetCells);
		isSolvable();

		calculateBestSolution();

		// validate cost
		int cost = 0;
		for (int i = 0; i < solutionPath.size() - 1; i++) {
			if (targetCells.contains(solutionPath.get(i))) {
				cost += instance.getActionCosts().get("suck");// suck operation
			}
			cost += shortestDistance(solutionPath.get(i),
					solutionPath.get(i + 1));
		}
		//		System.out.println("costo soluzione = " + solutionPath + " =" + cost);

		completeSolutionPath = buildCompletePath();

		solution = new ArrayList<Action>();
		buildSolutionFromVertexPath();

		long estimatedTime = System.currentTimeMillis() - startTime;

		//		System.out.println("\nelapsed time " + (double) (estimatedTime) / 1000
		//				+ " seconds");
	}

	public void solve() {
		long startTime = System.currentTimeMillis();

		//		instance.toString();
		board = createGraph();

		//shortesTpath = new FloydWarshallShortestPaths<Integer, DefaultEdge>(board);

		distances= new int [instance.getSize()*instance.getSize()][instance.getSize()*instance.getSize()];
		for (int i = 0; i < instance.getSize()*instance.getSize(); i++) {
			for (int j= 0; j < instance.getSize()*instance.getSize(); j++) {
				distances[i][j]=-1;
			}
		}
		
		targetCells = listAllreachableTarget();
		//		System.out.println("Target cells: " + targetCells);
		isSolvable();

		calculateBestSolution();

		// validate cost
		int cost = 0;
		for (int i = 0; i < solutionPath.size() - 1; i++) {
			if (targetCells.contains(solutionPath.get(i))) {
				cost += instance.getActionCosts().get("suck");// suck operation
			}
			cost += shortestDistance(solutionPath.get(i),
					solutionPath.get(i + 1));
		}
		//		System.out.println("costo soluzione = " + solutionPath + " =" + cost);

		completeSolutionPath = buildCompletePath();

		solution = new ArrayList<Action>();
		buildSolutionFromVertexPath();

		long estimatedTime = System.currentTimeMillis() - startTime;

		//		System.out.println("\nelapsed time " + (double) (estimatedTime) / 1000
		//				+ " seconds");
	}


	public void solveFromInstance(Instance i) {
		long startTime = System.currentTimeMillis();

		instance = i;
		//		instance.toString();
		board = createGraph();
		//shortesTpath = new FloydWarshallShortestPaths<Integer, DefaultEdge>(board);
		targetCells = listAllreachableTarget();
		//		System.out.println("Target cells: " + targetCells);
		isSolvable();

		calculateBestSolution();

		completeSolutionPath = buildCompletePath();

		solution = new ArrayList<Action>();
		buildSolutionFromVertexPath();

		long estimatedTime = System.currentTimeMillis() - startTime;

		//		System.out.println("\nelapsed time " + (double) (estimatedTime) / 1000
		//				+ " seconds");

	}

	private List<Integer> buildCompletePath() {
		List<Integer> completePath = new ArrayList<Integer>();

		for (int pathLoc = 0; pathLoc < solutionPath.size() - 1; pathLoc++) {

			List<DefaultEdge> tempStep;
			DijkstraShortestPath<Integer, DefaultEdge> pathFinder = new DijkstraShortestPath<Integer, DefaultEdge>(
					board, solutionPath.get(pathLoc),
					solutionPath.get(pathLoc + 1));
			tempStep = pathFinder.getPathEdgeList();

			// System.out.println("bestpath between "+bestPermutation.get(pathLoc)+" and "+bestPermutation.get(pathLoc+1));
			for (DefaultEdge defaultEdge : tempStep) {

				if (completePath.size() == 0
						|| !completePath.get(completePath.size() - 1).equals(
								(Integer) board.getEdgeSource(defaultEdge)))
					completePath
					.add((Integer) board.getEdgeSource(defaultEdge));

				completePath.add((Integer) board.getEdgeTarget(defaultEdge));

				// System.out.print((Integer)directedGraph.getEdgeSource(defaultEdge)+" "+(Integer)
				// directedGraph.getEdgeTarget(defaultEdge)+" " );

			}
			// System.out.println();

		}

		return completePath;
	}

	// two vertex at position i and i+1 are such that
	// abs(vertex(i),vertex(i+1))=1(left or right) or instance.size();(up and
	// down)
	private void buildSolutionFromVertexPath() {
		List<Integer> pulite = new ArrayList<Integer>();

		for (int i = 0; i < completeSolutionPath.size() - 1; i++) {
			int from = completeSolutionPath.get(i);
			int to = completeSolutionPath.get(i + 1);
			int diff = from - to;
			if (targetCells.contains(from) && !pulite.contains(from)
					&& solutionPath.contains(from)) {
				solution.add(new DynamicAction("suck"));
				pulite.add(from);
			}
			if (diff == 1) {// left
				solution.add(new DynamicAction("left"));
			} else if (diff == -1) {// right
				solution.add(new DynamicAction("right"));
			} else if (diff == instance.getSize()) {// up
				solution.add(new DynamicAction("up"));
			} else if (diff == -instance.getSize()) {// down
				solution.add(new DynamicAction("down"));

			}

		}// for
		if (instance.getBoardState()[instance.getBasePos().getX()][instance
		                                                           .getBasePos().getY()] == LocationState.Dirty) {
			solution.add(new DynamicAction("suck"));
		}
		solution.add(NoOpAction.NO_OP);

		//		System.out.println("\nSOLUZIONE:");
		for (Action soluAction : solution) {
			String name = ((DynamicAction) soluAction).getName();
			//			System.out.print(name + " ");
		}
		//		System.out.println();
	}

	/**
	 * Calcola iterativamente blocchi di K nodi migliori, e lio mette nella
	 * soluzione.
	 * 
	 */

	private void calculateBestSolution() {
		double solutionCost = 0;

		List<Integer> daPulire = new ArrayList<Integer>();
		daPulire.addAll(targetCells);

		solutionPath.add(
				0,
				RowMajIdx(instance.getAgentPos().getX(), instance.getAgentPos()
						.getY(), instance.getSize()));

		// finchè ho energia o ci sono ancora celle da pulire
		// Create the initial vector

		boolean end = false;
		while (solutionCost < instance.getEnergy() && daPulire.size() > 0
				&& !end) {
			//			System.out.println("Gruppo nuovo");
			ICombinatoricsVector<Integer> initialVector = Factory
					.createVector(daPulire);
			// provo gruppi dal piu grande al piu piccolo
			boolean continueSearch = true;
			List<Integer> bestGruppoIntermedio = new ArrayList<Integer>();
			for (int k = KBEST; k > 0 && continueSearch; k--) {
				// Create a simple combination generator to generate
				// K-combinations of the initial vector
				Generator<Integer> gen = Factory
						.createSimpleCombinationGenerator(initialVector, k);

				double minSizeCostStep = Integer.MAX_VALUE;
				for (ICombinatoricsVector<Integer> combination : gen) {
					List<Integer> combinazione = combination.getVector();
					// System.out.println(combinazione);
					double currCost = shortestDistance(
							solutionPath.get(solutionPath.size() - 1),
							combinazione.get(0));
					currCost += combinazione.size()
							* instance.getActionCosts().get("suck")
							.doubleValue();
					for (int pathLoc = 0; pathLoc < combination.getSize() - 1; pathLoc++) {
						currCost += shortestDistance(combinazione.get(pathLoc),
								combinazione.get(pathLoc + 1));
					}
					// affinchè sia uno step ammissibile bisogna controllare
					// che rimanga energia
					// per ritornare alla base
					int distanzaBase = shortestDistance(
							combinazione.get(combinazione.size() - 1),
							RowMajIdx(instance.getBasePos().getX(), instance
									.getBasePos().getY(), instance.getSize()));// dall'ultimo
					// nodo
					// dirty
					// pulito
					// alla
					// base
					// di
					// nuovo
					if (solutionCost + currCost + distanzaBase <= instance
							.getEnergy()) {// step ammissibile (dopo aver
						// visitato combinazione node riesco
						// a tornare alla base
						if (currCost < minSizeCostStep) {
							// step ammissibile e migliore di quello trovato
							// prima
							minSizeCostStep = currCost;
							bestGruppoIntermedio.clear();
							bestGruppoIntermedio.addAll(combinazione);
							//							System.out
							//									.println("nuova combo migliore "
							//											+ combinazione + "costo "
							//											+ minSizeCostStep);
						}
					}

				}// combinazioni k<=KBEST corrente

				if (bestGruppoIntermedio.size() > 0) {
					// trovato un gruppo di dim <=KBEST ammissibile, posso
					// smettere di cercare
					//					System.out
					//							.println("gruppo buono trovato k massimo possibile valuta possibilità di abbassare il threshold KBEST");
					//					System.out.println("DIM =" + k);
					continueSearch = false;
					solutionPath.addAll(bestGruppoIntermedio);
					daPulire.removeAll(bestGruppoIntermedio);
					//					System.out.println("nuovo target cell =" + daPulire);
					solutionCost += minSizeCostStep;
				}

			}// for KBEST

			if (bestGruppoIntermedio.size() <= 0) {
				// solutionPath.add(RowMajIdx(instance.getBasePos().getX(),instance.getBasePos().getY(),
				// instance.getSize()));
				//				System.out.println("finito");
				end = true;
			}

		}
		solutionPath.add(RowMajIdx(instance.getBasePos().getX(), instance
				.getBasePos().getY(), instance.getSize()));
		//		System.out.println("path complete =" + solutionPath);

	}

	private void isSolvable() {
		if (instance.getEnergy() < shortestDistance(
				RowMajIdx(instance.getAgentPos().getX(), instance.getAgentPos()
						.getY(), instance.getSize()),
						RowMajIdx(instance.getBasePos().getX(), instance.getBasePos()
								.getY(), instance.getSize())))

		{
			//			System.out.println("Not Solvable");
			//			System.out.println("EXITING");
			System.exit(-1);
		}

	}

	public ArrayList<Integer> listAllreachableTarget() {
		ArrayList<Integer> target = new ArrayList<Integer>();
		for (int i = 0; i < instance.getSize(); i++) {
			for (int j = 0; j < instance.getSize(); j++) {
				if (instance.getBoardState()[i][j] == LocationState.Dirty) {
					boolean unreachable = true;
					// controllo se reachABLE(almeno una distanza < MAX
					for (int k = 0; k < instance.getSize() && unreachable; k++) {
						for (int l = 0; l < instance.getSize() && unreachable; l++) {
							if (k != i && l != j) {// non la stessa cella
								if (shortestDistance(
										RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(k, l, instance.getSize())) < INFDIST) {
									unreachable = false;
								}
							}
						}
					}

					if (!unreachable) {
						target.add(RowMajIdx(i, j, instance.getSize()));
					}
				}
			}
		}
		return target;
	}

	private int RowMajIdx(int x, int y, int size) {
		return x * size + y;
	}

	private DirectedGraph<Integer, DefaultEdge> createGraph() {
		DirectedGraph<Integer, DefaultEdge> g = new DefaultDirectedGraph<Integer, DefaultEdge>(
				DefaultEdge.class);

		for (int i = 0; i < instance.getSize(); i++) {
			for (int j = 0; j < instance.getSize(); j++) {
				g.addVertex(new Integer(RowMajIdx(i, j, instance.getSize())));

			}
		}

		// rendo le celle ostacolo irraggiungibili. nessuna connnessione in
		// uscita ne in entrata
		// escludo gli angoli. Quando da una cella dentro il bordo creo un arco
		// automaticamente creo l'arco opposto
		for (int i = 0; i < instance.getSize(); i++) {
			for (int j = 0; j < instance.getSize(); j++) {
				// celle dentro il bordo esterno
				if (instance.getBoardState()[i][j] != LocationState.Obstacle) {
					if (i > 0 && i < instance.getSize() - 1) {
						if (j > 0 && j < instance.getSize() - 1) {
							if (instance.getBoardState()[i - 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i - 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i + 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i + 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i][j + 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j + 1, instance.getSize()));
							}

							if (instance.getBoardState()[i][j - 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j - 1, instance.getSize()));
							}
						}
					}

					// prima riga senza angoli
					if (i == 0) {
						if (j > 0 && j < instance.getSize() - 1) {
							if (instance.getBoardState()[i + 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i + 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i][j + 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j + 1, instance.getSize()));
							}

							if (instance.getBoardState()[i][j - 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j - 1, instance.getSize()));
							}
						}
					}

					// ultimariga senza angoli
					if (i == instance.getSize() - 1) {
						if (j > 0 && j < instance.getSize() - 1) {
							if (instance.getBoardState()[i - 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i - 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i][j + 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j + 1, instance.getSize()));
							}

							if (instance.getBoardState()[i][j - 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j - 1, instance.getSize()));
							}
						}
					}

					// prima colonna senza angoli
					if (j == 0) {
						if (i > 0 && i < instance.getSize() - 1) {
							if (instance.getBoardState()[i - 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i - 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i + 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i + 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i][j + 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j + 1, instance.getSize()));
							}

						}
					}

					// ultima colonna senza angoli
					if (j == instance.getSize() - 1) {
						if (i > 0 && i < instance.getSize() - 1) {
							if (instance.getBoardState()[i - 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i - 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i + 1][j] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i + 1, j, instance.getSize()));
							}

							if (instance.getBoardState()[i][j - 1] != LocationState.Obstacle) {
								g.addEdge(RowMajIdx(i, j, instance.getSize()),
										RowMajIdx(i, j - 1, instance.getSize()));
							}

						}
					}

				}// obstacle
			} // forj
		}// fori

		for (int i = 0; i < instance.getSize() * instance.getSize(); i++) {
			for (int j = 0; j < instance.getSize() * instance.getSize(); j++) {
				if (g.containsEdge(i, j)) {
					g.addEdge(j, i);
				}
			}
		}

		return g;
	}

	public static void main(String[] args) {
		VacuumAgentSolverHeuristicKBest agent = new VacuumAgentSolverHeuristicKBest();
		agent.solve("instance_EXAMPLE.xml");
	}

	public String getId() {
		return id;
	}

	@Override
	public void run() {
		this.solve();

	}

	public List<Action> getSolution() {
		return solution;
	}

}
